#include "placemedicine.h"
#include "simplelib.h"
uint32_t PM_num = 1; //1 或者 3
uint32_t PM_start_flag = 0;
uint32_t PM_wait_ms = 0;
uint32_t PM_cnt_ms = 0;
PLACEMEDICINE_STATUS place_medicine1, place_medicine3, next_PM_status;

#define LEFTID 6
#define RIGHTID 5
#define SCANID 7

void test_PM(int num)
{
    PM_num = num; //1 或者 3
    PM_start_flag = 1;
}

void PM_init()
{
    PosLoopCfg(CAN1, LEFTID, 0, 0, 600);
    uprintf("init LEFTID\r\n");
    PosLoopCfg(CAN1, RIGHTID, 0, 0, 600);
    uprintf("init RIGHTID\r\n");
    PosLoopCfg(CAN1, SCANID, 0, 0, 50);
    uprintf("init SCANID\r\n");
    steer_init();
    PM_start_flag = 0;
    place_medicine1 = OPEN_BOX;
    place_medicine3 = OPEN_BOX;
    next_PM_status = PLACEMEDICINE_NONE;
}

void PM_exe()
{
    if (PM_start_flag == 1)
    {
        PM_state_machine();
    }
}

void PM_state_machine()
{
    PLACEMEDICINE_STATUS *PM_status;
    static uint8_t first_func = 1;
    if (PM_num == 1)
    {
        PM_status = &place_medicine1;
    }
    else if (PM_num == 3)
    {
        PM_status = &place_medicine3;
    }
    else
        return;
    switch (*PM_status)
    {
    case OPEN_BOX:
        open_box();

        /* 等待1s进入下一状态 */
        *PM_status = PM_WAIT_STATUS;
        PM_wait_ms = 200;
        PM_cnt_ms = 0;
        next_PM_status = SEND_GRIP;
        break;
    case SEND_GRIP:
        if (first_func) /*只调用一次*/
        {
            first_func = 0;
            send_grip();
        }
        if (flag.clock_10ms_flag) /* 10ms查询一次电机位置 */
        {
            send_grip();
            flag.clock_10ms_flag = 0;
            if (PM_num == 1)
                ReadActualPos(CAN1, LEFTID);
            else
                ReadActualPos(CAN1, RIGHTID);
        }
        if (PM_num == 1)
        {
            if (grip_is_arrive(left_conveyor_belt_pos)) /* 判断电机是否到达目标位置 */
            {
                /* 等待1s进入下一状态 */
                *PM_status = PM_WAIT_STATUS;
                PM_wait_ms = 200;
                PM_cnt_ms = 0;
                next_PM_status = LOOSEN_GRIP;
                first_func = 1;
            }
        }
        else
        {
            if (grip_is_arrive(right_conveyor_belt_pos)) /* 判断电机是否到达目标位置 */
            {
                /* 等待1s进入下一状态 */
                *PM_status = PM_WAIT_STATUS;
                PM_wait_ms = 200;
                PM_cnt_ms = 0;
                next_PM_status = LOOSEN_GRIP;
                first_func = 1;
            }
        }
        break;
    case LOOSEN_GRIP:
        loose_grip();
        if (PM_num == 1)
            voice_play_1();
        else if (PM_num == 3)
            voice_play_2();

        /* 等待1s进入下一状态 */
        *PM_status = PM_WAIT_STATUS;
        PM_wait_ms = 200;
        PM_cnt_ms = 0;
        next_PM_status = CLOSE_GRIP;
        break;
    case CLOSE_GRIP:
        close_grip();

        /* 等待1s进入下一状态 */
        *PM_status = PM_WAIT_STATUS;
        PM_wait_ms = 000;
        PM_cnt_ms = 0;
        next_PM_status = RECIEVE_GRIP;
        break;
    case RECIEVE_GRIP:
        if (first_func) /*只调用一次*/
        {
            first_func = 0;
            recieve_grip();
        }
        if (flag.clock_10ms_flag) /* 10ms查询一次电机位置 */
        {
            recieve_grip();
            flag.clock_10ms_flag = 0;
            if (PM_num == 1)
                ReadActualPos(CAN1, LEFTID);
            else
                ReadActualPos(CAN1, RIGHTID);
        }
        if (grip_is_arrive(0)) /* 判断电机是否返回初始位置 */
        {
            /* 等待1s进入下一状态 */
            *PM_status = PM_WAIT_STATUS;
            PM_wait_ms = 000;
            PM_cnt_ms = 0;
            next_PM_status = CLOSE_BOX;
            first_func = 1;
        }
        break;
    case CLOSE_BOX:
        close_box();

        /* 等待1s进入下一状态 */
        *PM_status = PM_WAIT_STATUS;
        PM_wait_ms = 200;
        PM_cnt_ms = 0;
        next_PM_status = PLACEMEDICINE_NONE;
        if(first_to_go == 1 && PM_num == 1){
            scan(3);
        }
        if(first_to_go == 3 && PM_num == 3){
            scan(1);
        }
        break;
    case PM_WAIT_STATUS:
        if (PM_cnt_ms >= PM_wait_ms)
        {
            *PM_status = next_PM_status;
        }
        break;
    case PLACEMEDICINE_NONE:
        PM_start_flag = 0;
        break;
    default:
        PM_start_flag = 0;
        break;
    }
}

void open_box()
{
    if (PM_num == 1)
    {
        steer_set_angle(BOX_STEER_LEFT, LEFT_BOX_OPEN_ANGLE);
    }
    else if (PM_num == 3)
    {
        steer_set_angle(BOX_STEER_RIGHT, RIGHT_BOX_OPEN_ANGLE);
    }
}
void close_box()
{
    if (PM_num == 1)
    {
        steer_set_angle(BOX_STEER_LEFT, LEFT_BOX_CLOSE_ANGLE);
    }
    else if (PM_num == 3)
    {
        steer_set_angle(BOX_STEER_RIGHT, RIGHT_BOX_CLOSE_ANGLE);
    }
}

void loose_grip()
{
    if (PM_num == 1)
    {
        steer_set_angle(GRIP_STEER_LEFT, LEFT_GRIP_LOOSE_ANGLE);
    }
    else if (PM_num == 3)
    {
        steer_set_angle(GRIP_STEER_RIGHT, RIGHT_GRIP_LOOSE_ANGLE);
    }
}
void close_grip()
{
    if (PM_num == 1)
    {
        steer_set_angle(GRIP_STEER_LEFT, LEFT_GRIP_CLOSE_ANGLE);
    }
    else if (PM_num == 3)
    {
        steer_set_angle(GRIP_STEER_RIGHT, RIGHT_GRIP_CLOSE_ANGLE);
    }
}

void send_grip()
{
    if (PM_num == 1)
    {
        PosCrl(CAN1, LEFTID, ABSOLUTE_MODE, left_conveyor_belt_pos);
    }
    else if (PM_num == 3)
    {
        PosCrl(CAN1, RIGHTID, ABSOLUTE_MODE, right_conveyor_belt_pos);
    }
}
void recieve_grip()
{
    if (PM_num == 1)
    {
        PosCrl(CAN1, LEFTID, ABSOLUTE_MODE, 0);
    }
    else if (PM_num == 3)
    {
        PosCrl(CAN1, RIGHTID, ABSOLUTE_MODE, 0);
    }
}

/* 判断电机是否达到位置环目标位置 */
uint8_t grip_is_arrive(int target_pos)
{
    if (PM_num == 1)
    {

        if (fabs(Motor[LEFTID].pos - target_pos) < 200) //TODO
        {
            return 1;
        }
        else
        {
            return 0;
        }
    }
    else if (PM_num == 3)
    {
        if (fabs(Motor[RIGHTID].pos - target_pos) < 200) //TODO
        {
            return 1;
        }
        else
        {
            return 0;
        }
    }
    return 0;
}

int left_scan_pos = 1000; 
int right_scan_pos = -96000;
uint8_t scan(int num)
{
    if (num == 1)
    {
        PosCrl(CAN1, SCANID, ABSOLUTE_MODE, left_scan_pos);
    }
    else if (num == 3)
    {
        PosCrl(CAN1, SCANID, ABSOLUTE_MODE, right_scan_pos);
    }
    return 0;
}
uint8_t rescan()
{
    PosCrl(CAN1, SCANID, ABSOLUTE_MODE, 0);
    return 0;
}